#define _USE_MATH_DEFINES
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <iostream>
#include "Kalman.h"
#include <cmath>
#include <vector>
#include "ros/ros.h"


using namespace Eigen;
using namespace std;


Kalman::Kalman()
{
	 ROS_INFO("Kalman Constructor");
	 m_KM_initial=false;
	 fp_cov=fopen("cov.data","w");
     fprintf(fp_cov,"(0 0),(1 0),(2 0),(3 0),(0 1),(1 1),(2 1),(3 1),(0 2),(1 2),(2 2),(3 2),(0 3),(1 3),(2 3),(3 3)\n");
}

void Kalman::init()
{
    m_A(0,0)=1;
	m_A(1,0)=0;
	m_A(2,0)=0;
	m_A(3,0)=0;
	m_A(0,1)=dt;
	m_A(1,1)=1;
	m_A(2,1)=0;
	m_A(3,1)=0;
    m_A(0,2)=0;
	m_A(1,2)=0;
	m_A(2,2)=1;
	m_A(3,2)=0;
	m_A(0,3)=0;
	m_A(1,3)=0;
	m_A(2,3)=dt;
	m_A(3,3)=1;


	m_H(0,0)=1;
	m_H(1,0)=0;
	m_H(0,1)=0;
	m_H(1,1)=0;
	m_H(0,2)=0;
	m_H(1,2)=1;
	m_H(0,3)=0;
	m_H(1,3)=0;

	//m_Q=(1/100)*m_Q.Identity();
	m_Q=0.05*m_Q.Identity();
	//m_R=15*m_R.Identity();
        m_R=50*m_R.Identity();

	m_x=m_x.Zero();

	//m_P=30*m_P.Identity();
	m_P=100*m_P.Identity();
	m_KM_initial=false;
	m_LPF_initial=false;
	m_AVG_initial=false;
	m_MOVING_AVG_initial=false;
	m_total_num_samp=0;
	m_moving_num_samp=0;
}

Vector4f Kalman::process(Matrix<float, 2, 1> measure)
{
	 Matrix<float, 4, 1>xp;
	 Matrix<float, 4, 4>Pp;
	 Matrix<float, 4, 2>K;
	 Matrix<float, 2, 1>z;
	 Matrix<float, 2,2> temp;

	 
	 xp=m_A*m_x;
	 //cout<<xp<<endl;

	 Pp=m_A*m_P*m_A.transpose()+m_Q;
	 //cout<<Pp<<endl;

	 temp=m_H*Pp*m_H.transpose()+m_R;
	 //cout<<temp<<endl;

	 K=Pp*m_H.transpose()*temp.inverse();
	 //cout<<K<<endl;

	 z=measure;
	 m_x=xp+K*(z-m_H*xp);

	 //cout<<z<<"\n\n"<<m_x<<"\n\n";

	 
	 m_P=Pp-K*m_H*Pp;

	 //cout<<m_P<<"\n\n";

	 //fprintf(fp_cov,"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",m_P(0 ,0),m_P(1, 0),m_P(2, 0),m_P(3, 0),m_P(0, 1),m_P(1 ,1),m_P(2, 1),m_P(3 ,1),m_P(0, 2),m_P(1, 2),m_P(2, 2),m_P(3, 2),m_P(0, 3),m_P(1, 3),m_P(2 ,3),m_P(3 ,3));

	 #if 0
	 #endif

	 return m_x;
	 
}

Matrix<float, 2, 1> Kalman::LPF(Matrix<float, 2, 1> measure)
{
   if(!m_LPF_initial)
   {
     prevX_lpf=measure;
	 m_LPF_initial=true;
	 return prevX_lpf;
   }
   else
   {
	     lpf_x=alpha*prevX_lpf+(1-alpha)*measure;
		 prevX_lpf=lpf_x;
		 return lpf_x;
   }
}

Matrix<float, 2, 1> Kalman::AVG(Matrix<float, 2, 1> measure)
{
   if(!m_AVG_initial)
   {
     m_total_num_samp++;
	 prevX_avg=measure;
	 m_AVG_initial=true;
	 return prevX_avg;
   }
   else
   {
     m_total_num_samp++;
     float alp=(m_total_num_samp-1)/m_total_num_samp;
	 avg_x=alp*prevX_avg+(1-alp)*measure;
	 prevX_avg=avg_x;
	 return avg_x;   
   }
	
}

Matrix<float, 2, 1> Kalman::MOVING_AVG(Matrix<float, 2, 1> measure)
{
  return measure;
	
}
Kalman::~Kalman()
  {
  }
